import numpy as np
import pickle

fuck = 0

def my_map(query, candidate):
    mapping = -1 * np.ones((np.shape(query)[0]), dtype = np.int)
    query_size = np.shape(query)[0]
    last_match = 0
    for i in range(query_size):
        min_idx = -1
        min_dis = np.inf
        x1 = query[i][0]
        y1 = query[i][1]
        yaw1 = query[i][2]
        candidate_size = np.shape(candidate)[0]


        end = last_match + 1000
        end = end if end < candidate_size else candidate_size

        for j in range(last_match, end):
            x2 = candidate[j][0]
            y2 = candidate[j][1]
            yaw2 = candidate[j][2]
            dx = x1 - x2
            dy = y1 - y2
            dyaw = abs(yaw1 - yaw2)
            if dyaw > np.pi:
                dyaw = 2*np.pi - dyaw
            if dyaw > np.pi / 6:
                continue
            dis = np.sqrt(np.power(dx,2) + np.power(dy,2))
            if dis > 10: # 30度，10米时，全都可以在不同视频中找到对应帧
                continue
            if dis < min_dis:
                min_dis = dis
                min_idx = j
                last_match = j

        mapping[i] = min_idx
        if i%100 == 1:
            print(i)
    return mapping

round1 = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/round1/round1_sync_position.npy'
round2 = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/round2/round2_sync_position.npy'
round3 = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/round3/round3_sync_position.npy'

GNSS1 = np.load(round1)
GNSS2 = np.load(round2)
GNSS3 = np.load(round3)

query = GNSS1
candidate = GNSS2
map1 = -1 * np.ones((np.shape(query)[0],3), dtype = np.int)
mapping = my_map(query, candidate)
map1[:,1] = mapping
candidate = GNSS3
mapping = my_map(query, candidate)
map1[:,2] = mapping

query = GNSS2
candidate = GNSS1
map2 = -1 * np.ones((np.shape(query)[0],3), dtype = np.int)
mapping = my_map(query, candidate)
map2[:,0] = mapping
candidate = GNSS3
mapping = my_map(query, candidate)
map2[:,2] = mapping

query = GNSS3
candidate = GNSS1
map3 = -1 * np.ones((np.shape(query)[0],3), dtype = np.int)
mapping = my_map(query, candidate)
map3[:,0] = mapping
candidate = GNSS2
mapping = my_map(query, candidate)
map3[:,1] = mapping


data = (map1, map2, map3)
f = open('E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/IndexMap.pickle','wb')
pickle.dump(data, f)





exit(0)